﻿///
///  Filename:  KDTree.cs
///  Author:    Sérgio Severino, nº 9733
///   /*
///      Implementation:  10/06/2012
///   */
///

using System;

namespace KDTreeLib
{
    
    /// <summary>
    /// A classe KDTree
    /// </summary>
    /// <
    public class KDTree<T>
    {
        /// <summary>
        /// Uma classe interna auxiliar para o metodo do vizinho mais próximo
        /// </summary>
        private class NodeDist
        {
            public int node;
            public ulong distance;

            /// <summary>
            /// Construtor
            /// </summary>
            /// <param name="node">Um indice do nó</param>
            /// <param name="distance">Uma distância ao quadrado</param>
            public NodeDist(int node, ulong distance)
            {
                this.node = node;
                this.distance = distance;
            }
        }

        private const int EMPTY_NODE = -1;
        private const int NOT_DELETED_NODE = 0;
        private const int DELETED_NODE = 1;
        private const Point<T> EMPTY_COORDINATE = null;

        private const int NONE_CHILD = 0;
        private const int LEFT = 1;
        private const int RIGHT = 2;

        private Stack memPointer;

        public Point<T>[] coordinate { private set; get; }

        public int[] deletedNode { private set; get; }
        public int[] parentNode { private set; get; }
        public int[] leftChildNode { private set; get; }
        public int[] rightChildNode { private set; get; }

        public int rootNode { private set; get; }

        private int balanceCounter;
        private int balancePointer;

        /// <summary>
        /// O construtor principal
        /// </summary>
        /// <param name="treeSize">O tamanho da árvore</param>
        public KDTree(int treeSize)
        {
            this.balanceCounter = 0;
            this.balancePointer = 8;

            this.rootNode = KDTree<T>.EMPTY_NODE;

            if (treeSize > 0)
            {
                this.GenerateTree(treeSize);
            }
            else
                return;


        }

        /// <summary>
        /// Metodo que cria espaço na memória para todas as variaveis
        /// </summary>
        /// <param name="treeSize">O tamanho de memória necessário</param>
        private void GenerateTree(int treeSize)
        {
            this.memPointer = new Stack(treeSize);
            this.coordinate = new Point<T>[treeSize];
            this.deletedNode = new int[treeSize];
            this.parentNode = new int[treeSize];
            this.leftChildNode = new int[treeSize];
            this.rightChildNode = new int[treeSize];

            for (int k = 0; k < treeSize; k++)
            {
                this.memPointer.Push(k);
                this.deletedNode[k] = KDTree<T>.DELETED_NODE;
                this.coordinate[k] = KDTree<T>.EMPTY_COORDINATE;
                this.parentNode[k] = KDTree<T>.EMPTY_NODE;
                this.leftChildNode[k] = KDTree<T>.EMPTY_NODE;
                this.rightChildNode[k] = KDTree<T>.EMPTY_NODE;
            }
        }

        /// <summary>
        /// O segundo construtor
        /// </summary>
        /// <param name="list">A lista de pontos</param>
        /// <param name="treeSize">O tamanho da árvore</param>
        public KDTree(Point<T>[] list, int treeSize)
            : this(treeSize)
        {
            this.rootNode = this.AddNodeRec(list, KDTree<T>.EMPTY_NODE, 0);
        }

        /// <summary>
        /// O terceiro construtor
        /// </summary>
        /// <param name="list">A lista de pontos</param>
        public KDTree(Point<T>[] list)
            : this(list, list.Length) { }

        /// <summary>
        /// Metodo que constrói a árvore
        /// </summary>
        /// <param name="list">A lista de pontos</param>
        /// <param name="parent">O nó pai</param>
        /// <param name="depth">A profundidade do nó na árvore</param>
        /// <returns></returns>
        private int AddNodeRec(Point<T>[] list, int parent, int depth)
        {
            if (list == null)
                return -1;

            // Obtem-se o eixo
            int pointDimension = list[0].Length;
            int axis = depth % pointDimension;

            // escolher o ponto medio da lista
            Point<T>.Merge_sort(list, axis);
            int median = list.Length / 2;


            int location = this.Malloc(list[median]);

            this.coordinate[location] = list[median];

            this.Insert(location, parent, KDTree<T>.NONE_CHILD);

            this.leftChildNode[location] = this.AddNodeRec(
                this.SubArray(list, 0, median - 1), location, depth + 1);

            this.rightChildNode[location] = this.AddNodeRec(
                this.SubArray(list, median + 1, list.Length - 1), location, depth + 1);

            return location;
        }

        /// <summary>
        /// Metodo que aloca um espaço de memória para um nó
        /// </summary>
        /// <param name="point">O ponto a alocar</param>
        /// <returns>O endereço de memória alocado</returns>
        private int Malloc(Point<T> point)
        {
            int node = this.memPointer.Pop();

            if (node == KDTree<T>.EMPTY_NODE)
            {
                for (int i = 0; i < this.deletedNode.Length; i++)
                {
                    if (this.deletedNode[i] == KDTree<T>.DELETED_NODE)
                    {
                        this.ManualBalance();
                        node = this.memPointer.Pop();
                        break;
                    }
                }
            }

            if (node != KDTree<T>.EMPTY_NODE)
            {
                this.leftChildNode[node] = KDTree<T>.EMPTY_NODE;
                this.rightChildNode[node] = KDTree<T>.EMPTY_NODE;
                this.parentNode[node] = KDTree<T>.EMPTY_NODE;
                this.deletedNode[node] = KDTree<T>.NOT_DELETED_NODE;
                this.coordinate[node] = point;
            }
            return node;
        }

        /// <summary>
        /// Metodo que devolve um sub array de pontos
        /// </summary>
        /// <param name="array">O array</param>
        /// <param name="begin">O inicio</param>
        /// <param name="end">O fim</param>
        /// <returns>O novo array</returns>
        private Point<T>[] SubArray(Point<T>[] array, int begin, int end)
        {
            if (begin > end)
                return null;

            Point<T>[] result = new Point<T>[end - begin + 1];

            for (int i = begin, j = 0; i <= end; i++, j++)
                result[j] = array[i];


            return result;
        }

        public override string ToString()
        {
            return this.ToString(this.rootNode);
        }

        /// <summary>
        /// Metodo que cria uma string com base no nó
        /// </summary>
        /// <param name="node">O nó</param>
        /// <returns>A string que representa o nó</returns>
        private string ToString(int node)
        {
            if (node == KDTree<T>.EMPTY_NODE)
                return "e";


            string str = "(";
            str += this.ToString(this.leftChildNode[node]) + ", ";

            if (this.deletedNode[node] == KDTree<T>.NOT_DELETED_NODE)
            {
                str += node + ": " + this.coordinate[node].ToString();
            }
            else
                str += "d";

            str += ", " + this.ToString(this.rightChildNode[node]) + ")";

            return str;
        }

        /// <summary>
        /// Método que cria uma representação, detalhada da árvore, em string
        /// </summary>
        /// <returns>A string</returns>
        public string ToStringDetails()
        {
            string info = "";

            for (int k = 0; k < this.coordinate.Length; k++)
            {
                if (k < this.coordinate.Length && k > 0)
                    info += "\n\n";
                info += ("x: " + k + ", root: " + this.rootNode + "\n");
                info += ("Parent: " + this.parentNode[k] + "\n");

                string str = "";

                if (this.coordinate[k] == KDTree<T>.EMPTY_COORDINATE)
                    str += "empty";
                else if (this.deletedNode[k] == KDTree<T>.DELETED_NODE)
                    str += "deleted";
                else
                {
                    str += "[";
                    for (int i = 0; i < this.coordinate[k].Length; i++)
                    {
                        str += (i == 0 ? "" : ", ");
                        str += this.coordinate[k][i].ToString();
                    }
                    str += "]";
                }

                info += ("Coordenate: " + str + "\n");
                info += ("Left: " + this.leftChildNode[k] + ", Right: " + this.rightChildNode[k]);
            }
            return info;

        }
        
        /// <summary>
        /// Método que insere um ponto na árvore
        /// </summary>
        /// <param name="node">O endereço do nó</param>
        /// <param name="parent">O nó pai</param>
        /// <param name="sideChild">O lado do qual o nó é filho</param>
        private void Insert(int node, int parent, int sideChild)
        {
            this.parentNode[node] = parent;

            if (sideChild == KDTree<T>.RIGHT)
                this.rightChildNode[parent] = node;
            else if (sideChild == KDTree<T>.LEFT)
                this.leftChildNode[parent] = node;

        }

        /// <summary>
        /// Método que adiciona o argumento à árvore
        /// </summary>
        /// <param name="point">Um ponto</param>
        /// <returns>O endereço onde o ponto foi inserido</returns>
        public int Add(Point<T> point)
        {
            this.balanceCounter++;
            
            if (this.balanceCounter >= this.balancePointer)
                this.ManualBalance();

            if (point == KDTree<T>.EMPTY_COORDINATE)
                return -1;

            int node = this.Malloc(point);

            if (node == KDTree<T>.EMPTY_NODE)
                return node;

            if (this.rootNode == KDTree<T>.EMPTY_NODE)
            {
                this.rootNode = node;
                this.coordinate[node] = point;

                return node;
            }

            int axis = 0;
            int parent = this.rootNode;
            int position = this.rootNode;
            int sideChild = KDTree<T>.NONE_CHILD;

            while (position != KDTree<T>.EMPTY_NODE)
            {
                parent = position;

                if (point.FirstIsSmaller(this.coordinate[position], axis))
                {
                    position = this.leftChildNode[position];
                    sideChild = KDTree<T>.LEFT;
                }
                else
                {
                    position = this.rightChildNode[position];
                    sideChild = KDTree<T>.RIGHT;
                }

                axis = (axis + 1) % point.Length;
            }

            this.Insert(node, parent, sideChild);
            return node;
        }

        /// <summary>
        /// Método que balanceia a árvore (reconstrói a árvore)
        /// </summary>
        public void ManualBalance()
        {
            this.balanceCounter = 0;

            int treeSize = this.coordinate.Length;
            int listSize = 0;

            for (int i = 0; i < this.coordinate.Length; i++)
            {
                if (this.coordinate[i] != KDTree<T>.EMPTY_COORDINATE
                    && this.deletedNode[i] == KDTree<T>.NOT_DELETED_NODE)
                    listSize++;
            }


            Point<T>[] list = new Point<T>[listSize];


            for (int i = 0, f = 0; i < this.coordinate.Length; i++)
            {
                if (this.coordinate[i] != KDTree<T>.EMPTY_COORDINATE
                    && this.deletedNode[i] == KDTree<T>.NOT_DELETED_NODE)
                {
                    list[f] = this.coordinate[i];
                    f++;
                }
            }

            this.rootNode = KDTree<T>.EMPTY_NODE;

            this.GenerateTree(treeSize);


            this.rootNode = this.AddNodeRec(list, KDTree<T>.EMPTY_NODE, 0);

        }

        /// <summary>
        /// Método que pesquisa o argumento na árvore
        /// </summary>
        /// <param name="point">O ponto a pesquisar</param>
        /// <returns>O endereço do ponto</returns>
        public int Search(Point<T> point)
        {
            int axis = 0;

            return this.SearchChilds(point, this.rootNode, axis);
        }

        /// <summary>
        /// Método que pesquisa um ponto na árvore até chegar a uma folha
        /// </summary>
        /// <param name="point">O ponto a pesquisar</param>
        /// <param name="node">O nó</param>
        /// <param name="axis">O eixo de comparação</param>
        /// <returns>O indice do ponto na árvore</returns>
        private int SearchChilds(Point<T> point, int node, int axis)
        {
            if (node == KDTree<T>.EMPTY_NODE ||
                (point == this.coordinate[node] && this.deletedNode[node] == KDTree<T>.NOT_DELETED_NODE))
                return node;

            if (point.FirstIsSmaller(this.coordinate[node], axis))
                return this.SearchChilds(point, this.leftChildNode[node], (axis + 1) % point.Length);
            else
                return this.SearchChilds(point, this.rightChildNode[node], (axis + 1) % point.Length);

        }

        /// <summary>
        /// Método que devolve um ponto no endereço do argumento
        /// </summary>
        /// <param name="node">O endereço do ponto</param>
        /// <returns></returns>
        public Point<T> GetPointAt(int node)
        {
            if (node >= this.coordinate.Length || node < 0
                    || this.deletedNode[node] == KDTree<T>.DELETED_NODE)
                return KDTree<T>.EMPTY_COORDINATE;
            else
                return this.coordinate[node];
        }

        /// <summary>
        /// Liberta memória do endereço do argumento
        /// </summary>
        /// <param name="node">O endereço de memória</param>
        private void Free(int node)
        {
            this.memPointer.Push(node);
        }

        /// <summary>
        /// Remove o ponto, do endereço de memória do argumento, da árvore
        /// </summary>
        /// <param name="node">O endereço do ponto</param>
        public void Delete(int node)
        {
            this.deletedNode[node] = KDTree<T>.DELETED_NODE;
        }
		
		/// <summary>
		/// Pesquisa o vizinho mais próximo do argumento
		/// </summary>
		/// <param name="point">O ponto</param>
		/// <returns>O endereço do vizinho mais próximo</returns>
        public int NearestNeighbor(Point<T> point)
		{
            ulong distance = ulong.MaxValue;
			int axis = 0;
			
			NodeDist a = this.NearestNeighbor( point, this.rootNode, axis, distance );
			distance = a.distance;
			//Console.WriteLine(distance);
			return a.node;
		}

        /// <summary>
        /// Pesquisa o vizinho mais próximo do argumento
        /// </summary>
        /// <param name="point">O ponto de referência</param>
        /// <param name="node">O endereço do nó actual</param>
        /// <param name="axis">O eixo de comparação</param>
        /// <param name="distance">A distância do nó pai</param>
        /// <returns></returns>
        private NodeDist NearestNeighbor(Point<T> point, int node, int axis, ulong distance)
		{
            if (node == KDTree<T>.EMPTY_NODE)
                return new NodeDist(node, ulong.MaxValue );

            ulong newDistance = point.Distance(this.coordinate[node]);

            NodeDist nodeAndDistance;

            // folha
            if (this.rightChildNode[node] == KDTree<T>.EMPTY_NODE
                && this.leftChildNode[node] == KDTree<T>.EMPTY_NODE)
            {
                if (newDistance < distance)
                    return new NodeDist(node, newDistance );
                else
                    return new NodeDist(KDTree<T>.EMPTY_NODE, distance);
            } 
            else // não folha
            {
                int side;
                //desce na árvore
                if (point.FirstIsSmaller(this.coordinate[node], axis))
                {
                    nodeAndDistance = this.NearestNeighbor(
                                                point,
                                                this.leftChildNode[node],
                                                (axis + 1) % point.Length,
                                                distance);
                    side = KDTree<T>.LEFT;
                }
                else
                {
                    nodeAndDistance = this.NearestNeighbor(
                                                point,
                                                this.rightChildNode[node],
                                                (axis + 1) % point.Length,
                                                distance);
                    side = KDTree<T>.RIGHT;
                }


                if (this.deletedNode[node] == KDTree<T>.NOT_DELETED_NODE && newDistance < nodeAndDistance.distance)
                    nodeAndDistance = new NodeDist( node, newDistance );


                NodeDist nodeAndDistanceSide = new NodeDist(KDTree<T>.EMPTY_NODE, int.MaxValue);

                // testa o outro ramo
                if (nodeAndDistance.distance > (ulong)(this.coordinate[node][axis] * this.coordinate[node][axis]))
                {
                    if (side == KDTree<T>.RIGHT)
                        nodeAndDistanceSide = this.NearestNeighbor(
                                                point,
                                                this.leftChildNode[node],
                                                (axis + 1) % point.Length,
                                                nodeAndDistance.distance);
                    else
                        nodeAndDistanceSide = this.NearestNeighbor(
                                                point,
                                                this.rightChildNode[node],
                                                (axis + 1) % point.Length,
                                                nodeAndDistance.distance);
                }

                if (nodeAndDistanceSide.distance < nodeAndDistance.distance)
                    nodeAndDistance = nodeAndDistanceSide;

                return nodeAndDistance;
            }
		}

    }

}
